#include "LQ_demo.hpp"

/*LLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLLL
 * @函数名称：void MotorDemo()
 * @功能说明：电机功能测试
 * @参数说明：无
 * @函数返回：无
 * @调用方法：MotorDemo();
 * @备注说明：使电机每隔 2 秒转动方向翻转一次
 QQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQ*/
void MotorDemo()
{
    /* 电机PWM */
    SetPWM LMotorPWM(PWM2, 100000, 30000, "inversed"); // SPI2_MISO gpio66
    SetPWM RMotorPWM(PWM1, 100000, 30000, "normal"); // SPI2_MOSI gpio65
    /* 电机方向GPIO */
    HWGpio LMotoDir(74, GPIO_Mode_In); // CAN3_RX  left motor dir
    HWGpio RMotoDir(75, GPIO_Mode_In); // CAN3_TX  right motor dir
    // 设置占空比
    LMotorPWM.SetDutyCycle(20000);
    RMotorPWM.SetDutyCycle(20000);
    // 使能
    LMotorPWM.Enable();
    RMotorPWM.Enable();

    while(true)
    {
        // LMotoDir.SetGpioValue(1);
        // RMotoDir.SetGpioValue(0);
        // sleep(2);
        LMotoDir.SetGpioValue(0);
        RMotoDir.SetGpioValue(1);
        sleep(2);
    }
}

// #include <main.hpp>
// int main(){
//       /* 电机PWM */
//     SetPWM LMotorPWM(PWM2, 100000, 30000, "inversed"); // SPI2_MISO gpio66
//     SetPWM RMotorPWM(PWM1, 100000, 30000, "normal"); // SPI2_MOSI gpio65
//     /* 电机方向GPIO */
//     HWGpio LMotoDir(74, GPIO_Mode_Out); // CAN3_RX  left motor dir
//     HWGpio RMotoDir(75, GPIO_Mode_Out); // CAN3_TX  right motor dir
//     // 设置占空比
//     LMotorPWM.SetDutyCycle(35000);
//     RMotorPWM.SetDutyCycle(35000);
//     // 使能
//     LMotorPWM.Enable();
//     RMotorPWM.Enable();

//     while(true)
//     {
//         // LMotoDir.SetGpioValue(1);
//         // RMotoDir.SetGpioValue(0);
//         // sleep(2);
//         LMotoDir.SetGpioValue(0);
//         RMotoDir.SetGpioValue(1);
//         sleep(2);
//     }
// }